//******************************************************************************
//FireWork Launcher
//Version:  1.0   Last edited: 22.01.2012
//Comments: implementation of Fireworks Launcher End Device
//          
//
//Authors:  Denis Surkes  ID 321416505
//          Ran Levy      ID 036679215
//          Ofer Simon    ID 039686951
//          Ela Niazov    ID 310259205

//******************************************************************************

#include "bsp.h"
#include "mrfi.h"
#include "bsp_leds.h"
#include "bsp_buttons.h"
#include "nwk_types.h"
#include "nwk_api.h"
#include "nwk_frame.h"
#include "nwk.h"

#include "msp430x22x4.h"
#include "vlo_rand.h"
#include <stdio.h>
#define MAX_LAUNCHES 31 //maximum number of launches premitted
#define TASK_NUMBER 5
#define ENTER 0x0D
#define H_THRESH 250 //80.0 degrees
#define ACTION_MSG_SIZE 8
#define BUTTON BIT2
#define TIMEOUT 10
#define TIMER_B_ONE_SEC 11650
/* Structs and enums */
/* indicates the priorities of the tasks they are set here*/
typedef enum Priorities{E_RUN_UART=0,E_RUN_TMP_INPUT=2,E_RUN_LAUNCH=1,E_LISTEN_JOIN=3,E_LISTEN_FRAME=4}Priorities;
/* indicates the menu type to be displayed next and the state of the machine */
typedef enum DisplayType{E_MAIN,E_CONF,E_TEST,E_OTHER,E_LAUNCH,E_,E_MAN,E_CONFMODE} DisplayType;
/* indicates the character that was input from hyper terminal */
typedef enum CharCode{E_C,E_L,E_M,E_T,E_N,E_P,E_S,E_A,E_ENTER} CharCode;
/* the states of the tasks */
typedef enum statesType{E_READY=0,E_SUSPENDED,E_BLOCKED,E_RUNNING}statesType;
/* the data structure for tasks. priority is not used, the place in the array represents the priority */
typedef struct TaskInfo
{
  void (*funcPtr)(void);
  int priority;
  statesType state;
}TaskInfo;
/* this structure holds the plan data and state */
typedef struct PlanData
{
  int PlanArray[MAX_LAUNCHES]; //array of launch plan times (secudnen)
  int ArrInd; // while creating plan- this is the ind pointer that fills the array. while running the plan, this points to current missle (also sent to launcher)
  int ArrSize; //plan size
  int inputStage; // -1 empty 0-total 1-intervals 2-finished
  int tempInput; // holds a temporary character for when we get inputs
  int minInput; // helps preventing an error when ':' inserted for timimngs
}PlanData;


/* Function declarations */
int numToChar(char *str, int n); // integer to char, str should be malloced anbd long enough
void menu(DisplayType type); // will print menu depends on the display type
void decideAction(); //will decide which action should be taken according to menu state and character input
void getInput(); // getting teh input of a char and giving it the correct CharCode
void newplan(); //  when begining a plan we launch this function
void inputPlan(); // in the middle of plan configuration. proccessing all plan stages.
void triggerLaunch(); //manual launch. when here the next missile is launched.
void printPlan(); // printing the current plan. in the flash memory. on begin (after IAR loads) it will be empty
void beginLaunch(uint8_t runMode); //function init start launching. run mode will tell us if its 0- manual,1-test,2-regular
void Launch(); // launch will be called every second to check for launch.
void TXString( char* string, int length ); // prints a string to hyperterminal
void MCU_Init(void); // initializing timers and UART 9600 BAUD
void Scheduler(); //the scheduler of the system. runs the first ready task according to priorities. suspends the task after run.
void runLaunch(void); //the task that the scheduler launches when in launch mode (both test and regular)
void runUART(void); // the task that the scheduler launches when UART is recieved.
void runTmpInput(void); // the task that the scheduler launches to process input
void Port_Init(); // initializing the ports for reset button
void setReadyTask(Priorities task); // sets a task to ready. according to name.
void listenFrame(); // a task for recieving a frame. to proccess temprature.
void listenJoin(); // listen join. the system always try to check for joins, bypasses scheduler because of performance issues.
void SendFrame(); // sending a launch frame ("launch:X") X indicates the launch number
void abort(); // locking the system. the system no longer works need to reset both launcher and base after this with button. 
void TaskInit(); // initilizing tasks for scheduler
void createRandomAddress(); // creates a random address for wireless
void SendAbortFrame(); // sending an abort to launcher ("abort: ");
/* Global variables */
DisplayType gCurrentState; // the current menu state
CharCode gInput; // the current character code from uart
char gRx; // the char we get from UART
PlanData gPd;// holds the plan data

TaskInfo taskArr[TASK_NUMBER];      //Task status array
static uint8_t gSleepSem;          //0 - scheduler is sleeping. when this is above 1 it means 1 task or more is in ready state.
static uint8_t gSystemAbort;       //1 - system abort indicator. everything is locked in this state
static uint8_t gScbFlag;           //if on abort mode. wireless signals are ignored

int gTimerBCnt; // counting seconds every second this counter is increases only active on launch or test mode
uint8_t gRunMode; // 0- no run or manual run 1- test run 2- real launch

int gTimeout;// for wireless bit test. and for temprature recieve.
uint8_t gIsConnected; // this status represents launcher connection.
char* gErrorMessage =  "invalid input\n\r"; // when an invalid char is entered
char* gLaunchMsg = "launch:"; // launch frame (could not be constant cannot send const to simplicity for some reason
char* gAbortMsg = "abort: "; // abort frame (could not be constant same reason)
__no_init volatile int Flash_memory[MAX_LAUNCHES+4] @ 0x1040; // flash memory. we will write down the configuration to this memory + the planData info
__no_init volatile char Flash_Addr[4] @ 0x10F0; // holds the wireless address.


__no_init volatile int tempOffset @ 0x10F4; // Temperature offset set at production
// reserve space for the maximum possible peer Link IDs
static linkID_t sLID[NUM_CONNECTIONS];
static uint8_t  sNumCurrentPeers;

// callback handler
static uint8_t sCB(linkID_t); //modified to ready appropriate tasks. and disabling it when neccesery

// work loop semaphores
static uint8_t sPeerFrameSem; // when frame is recieved
static uint8_t sJoinSem; // when join token is recieved

// mode data verbose = default, deg F = default
char verboseMode = 1;
char degCMode = 0;

void main (void)
{
    addr_t lAddr;
  WDTCTL = WDTPW + WDTHOLD;                 // Stop WDT
  {
  // delay loop to ensure proper startup before SimpliciTI increases DCO
  // This is typically tailored to the power supply used, and in this case
  // is overkill for safety due to wide distribution.
    volatile int i;
    for(i = 0; i < 0xFFFF; i++){}
  }
  if( CALBC1_8MHZ == 0xFF )                 // Do not run if cal values are erased
  {
    volatile int i;
    P1DIR |= 0x03;
    BSP_TURN_ON_LED1();
    BSP_TURN_OFF_LED2();
    while(1)
    {
      for(i = 0; i < 0x5FFF; i++){}
      BSP_TOGGLE_LED2();
      BSP_TOGGLE_LED1();
    }
  } 
  // used for simplicity
  P2DIR = 0x27;
  P2OUT = 0x00;
  P3DIR = 0xC0;
  P3OUT = 0x00;
  P4DIR = 0xFF;
  P4OUT = 0x00;
  
  BSP_Init();
  
  if( Flash_Addr[0] == 0xFF && 
      Flash_Addr[1] == 0xFF && 
      Flash_Addr[2] == 0xFF && 
      Flash_Addr[3] == 0xFF )
  {
    createRandomAddress();                  // set Random device address at initial startup
  }
  lAddr.addr[0]=Flash_Addr[0];
  lAddr.addr[1]=Flash_Addr[1];
  lAddr.addr[2]=Flash_Addr[2];
  lAddr.addr[3]=Flash_Addr[3];
  SMPL_Ioctl(IOCTL_OBJ_ADDR, IOCTL_ACT_SET, &lAddr);
  
  MCU_Init();

  SMPL_Init(sCB);
  
  // network initialized


 Port_Init();
 TaskInit();
  gCurrentState= E_MAIN;
//  //reset values
  gPd.ArrSize=0;
  gPd.ArrInd=0;
  gPd.inputStage=0;
  gPd.tempInput=-1;
  gSystemAbort=0;
  gIsConnected=0;
  menu(gCurrentState);  
  // main while 1 loop. a bit different from launcher, no need to CPUOFF- power is not an issue
  while(1)
  {
    listenJoin();
    if(gSleepSem)
      Scheduler(0);
  }
}
void Scheduler()
{
  int index;
  uint8_t taskLaunched = 0;
  __disable_interrupt();
  //launch task by order in task Array (Task in Ready state with the smalest index) 
  for (index = 0; index < TASK_NUMBER && !taskLaunched; index++)
  {
    if (taskArr[index].state == E_READY)
    {
      taskArr[index].funcPtr();
      gSleepSem--;
      taskArr[index].state = E_SUSPENDED;
      taskLaunched = 1;
    }
  }
    __enable_interrupt();
}

void TXString( char* string, int length )
{
  int pointer;
  for( pointer = 0; pointer < length; pointer++)
  {
    volatile int i;
    UCA0TXBUF = string[pointer];
    while (!(IFG2&UCA0TXIFG));              // USCI_A0 TX buffer ready?
  }
}

void MCU_Init()
{
  BCSCTL1 = CALBC1_8MHZ;                    // Set DCO
  DCOCTL = CALDCO_8MHZ;
  
  BCSCTL3 |= LFXT1S_2;                      // LFXT1 = VLO
  
  TACCTL0 &= ~CCIE;                           // TACCR0 interrupt enabled
  TACCR0 = 1100;                           // ~0.25 second
  TACTL = TASSEL_1 + MC_1;                  // ACLK, upmode
  
  TBCCTL0 = CCIE;                           // TBCCR0 interrupt enabled
  TBCCR0 = TIMER_B_ONE_SEC;                 // ~1 second,(very close)
  TBCTL = TBSSEL_1 + MC_1;                  // ACLK, upmode
  
   /* UART initialize and configuration */
  P3SEL |= 0x30;                            // P3.4,5 = USCI_A0 TXD/RXD
  UCA0CTL1 = UCSSEL_2;                      // SMCLK
  UCA0BR0 = 0x41;                           // 9600 from 8Mhz
  UCA0BR1 = 0x3;
  UCA0MCTL = UCBRS_2;                       
  UCA0CTL1 &= ~UCSWRST;                     // **Initialize USCI state machine**
  IE2 |= UCA0RXIE;                          // Enable USCI_A0 RX interrupt
  __enable_interrupt(); 
}
/*------------------------------------------------------------------------------
* Runs in ISR context. Reading the frame should be done in the
* application thread not in the ISR thread.
------------------------------------------------------------------------------*/
static uint8_t sCB(linkID_t lid)
{
   __disable_interrupt();
  gScbFlag = 1;
  if(!gSystemAbort)
  {
    if (lid)
    {
      sPeerFrameSem++;
	  taskArr[E_LISTEN_FRAME].state = E_READY;
	  TACCTL0 = CCIE;
      gSleepSem++;
     // __bic_SR_register_on_exit(LPM3_bits);
    }
    else
    {
      sJoinSem++;
    }
  }
  __enable_interrupt();
  return 0;
}

void runTmpInput()
{
  
  
}


/*------------------------------------------------------------------------------
* ADC10 interrupt service routine
------------------------------------------------------------------------------*/
#pragma vector=ADC10_VECTOR
__interrupt void ADC10_ISR(void)
{
  __bic_SR_register_on_exit(CPUOFF);        // Clear CPUOFF bit from 0(SR)
}

/*------------------------------------------------------------------------------
* Timer A0 interrupt service routine
------------------------------------------------------------------------------*/
#pragma vector=TIMERA0_VECTOR
__interrupt void Timer_A (void)
{
  //This interupt turn on the CPU
  if (gScbFlag)
  {
    TACCTL0 &= ~CCIE;
    gScbFlag=0;
  }
  __bic_SR_register_on_exit(LPM3_bits);        // Clear LPM3 bit from 0(SR)
}

/*------------------------------------------------------------------------------
* USCIA interrupt service routine
------------------------------------------------------------------------------*/
#pragma vector=USCIAB0RX_VECTOR
__interrupt void USCI0RX_ISR(void)
{
  gRx = UCA0RXBUF;
  //Run UART Hendler
	setReadyTask(E_RUN_UART);
  __bic_SR_register_on_exit(LPM3_bits);        // Clear LPM3 bit from 0(SR)
  
  
}

void runUART()
{
  if (gCurrentState == E_MAN)
  {
    __disable_interrupt();
    getInput(); //force getinput to running
    __enable_interrupt();
  }
  else if (gCurrentState==E_CONFMODE)
  {
    inputPlan();
    
    // next step in plan(check rx, insert to array, inc counter) 
    
    //planInput; // this method check the int, (total first & then get all intervals)
  }
  else
  {
    getInput();
    // change get input to ready
  }
}

#pragma vector=TIMERB0_VECTOR
__interrupt void Timer_B (void)
{
  gTimeout++;
  if (gTimeout>TIMEOUT && gIsConnected)
  {
    gTimeout=0;
    gIsConnected=0;
    TXString("LAUNCHER DISCONNECTED!\n\r",24);
  }
  if (gRunMode !=0)
    setReadyTask(E_RUN_LAUNCH);
  __bic_SR_register_on_exit(LPM3_bits);        // Clear LPM3 bit from 0(SR)

}
/*
RESET button interrupt.
*/
#pragma vector=PORT1_VECTOR
__interrupt void Port1_ISR(void)
{
  if(P1IFG & BUTTON)
  {
    TXString("Restarting...\n\r",15);
    P1IFG &= ~BUTTON;
    WDTCTL = WDTPW + WDTCNTCL + WDTSSEL;    //System restart
    while(1)
    {}      
  }
  __bic_SR_register_on_exit(CPUOFF);        // Clear CPUOFF bit from 0(SR)
}
void menu(DisplayType type)
{
  switch (type)
  {
  case E_MAIN:
    TXString("\r\n(C)onfig\r\n(T)est\r\nStart (L)aunch\r\n(S)tart Manual Launch\r\n",59);
    break;    
  case E_CONF:
     TXString("\r\n(N)ew launch plan\r\n(P)rint current plan\r\n(M)ain menu\r\n",56);
    break;
    
  case E_TEST:
     TXString("\r\n(S)tart test\r\n(M)ain menu\r\n",29);
    break;
    
  case E_OTHER:
    break;
    
  case E_LAUNCH:
     TXString("\r\n(S)witch to Manual launch\r\n(A)bort launch and lock system\r\n",63);
    break;
    
  case E_MAN:
     TXString("\r\n(L)aunch next missile\r\n(M)ain menu\r\n",38);
    break;
  }
}
void getInput()
{
  if (gRx == 'C' || gRx == 'c')
    gInput = E_C;
 
  if (gRx == 'T' ||  gRx == 't')
    gInput = E_T;
 
  if (gRx == 'L' || gRx == 'l')
    gInput = E_L;
 
  if (gRx == 'M' || gRx == 'm')
    gInput = E_M;
 
  if (gRx == 'N' ||  gRx == 'n')
    gInput = E_N;
 
  if (gRx == 'P' ||  gRx == 'p')
    gInput = E_P;
 
  if (gRx == 'S' || gRx == 's')
    gInput = E_S;
 
  if (gRx == 'A' || gRx == 'a')
    gInput = E_A;
  if (gRx == ENTER)
    gInput = E_ENTER;
  
  decideAction();
}
void decideAction()
{
  switch(gCurrentState)
  {
  case E_LAUNCH:
    switch (gInput)
    {
    case E_S://switch to manual Launch
      gRunMode=0;
      
      gCurrentState = E_MAN;
      menu(E_MAN);
      break;
    case E_A: // abort Launch and go to main menu
      gRunMode=0;
      abort();
      break;
    case E_ENTER:
      menu(gCurrentState);
      break;
    }
    break;
    
  case E_MAIN:
    switch (gInput)
    {
    case E_C://config
      gCurrentState = E_CONF;
      menu(gCurrentState);
      break;
    case E_T: // testing mode
       gCurrentState = E_TEST;
      menu(gCurrentState);
      break;
    case E_L://auto- launch mode
      gCurrentState = E_LAUNCH;
      menu(gCurrentState);
      beginLaunch(2);
      break;
    case E_S: //start manual Launch mode
      gCurrentState = E_MAN;
      menu(gCurrentState);
      beginLaunch(0);
      break;
    case E_ENTER:
      menu(gCurrentState);
      break;
    }
    break;
    
  case E_CONF:
    switch (gInput)
    {
    case E_N: // new plan
      gCurrentState = E_CONFMODE;
      newplan(); 
      break;
    case E_P: // print plan
      printPlan(); 
      menu (gCurrentState);
      break;
    case E_M: // main menu
      gCurrentState =E_MAIN;
      menu (gCurrentState);
      break;
    case E_ENTER:
      menu(gCurrentState);
      break;
    } 
    break;
    
  case E_TEST:
    switch (gInput)
    {
    case E_S: // start test
     beginLaunch(1);       
      break;
    case E_M: // main menu
      gCurrentState=E_MAIN;
      menu(gCurrentState);
      break;
    case E_ENTER:
      menu(gCurrentState);
      break;
    }
    break;
    
  case E_MAN:
    switch (gInput)
    {
    case E_L: // launch next
    triggerLaunch(); // trigger flag is now 1.
      break;
    case E_M: // main menu
      gCurrentState = E_MAIN;
      menu(gCurrentState);
      break;
    case E_ENTER:
      menu(gCurrentState);
      break;
    }
    break;
  }
}
void printPlan()
{
  char pBuf[40];
  int size;
  /* load from flash memory */
  gPd.ArrSize = Flash_memory[0];
  gPd.inputStage = Flash_memory[1];
  for(int i=0;i<gPd.ArrSize;i++)
    gPd.PlanArray[i] = Flash_memory[i+2];
  
  if (gPd.inputStage==2)
  {
    int min;
    int sec;
     size = sprintf(pBuf,"printing the plan\n\r");
     TXString(pBuf,size);
    for (int i=0;i<gPd.ArrSize;i++) 
    {
      min= gPd.PlanArray[i]/60;
      sec= gPd.PlanArray[i]%60;
      
      size=sprintf(pBuf,"Launch #");
      TXString(pBuf,size);
      
      size=numToChar(pBuf,i);
      TXString(pBuf,size);
      
      TXString("- ",2);
      
      size=numToChar(pBuf,min);
     TXString(pBuf,size);
     
      TXString(":",1);
      if (sec<10) TXString("0",1);
 
     size=numToChar(pBuf,sec);
      TXString(pBuf,size);
      
      TXString("\n\r",2);
    }
  }
  else
  {
      size = sprintf(pBuf,"please make a plan before trying to print one.. DUH...!!!\n\r");
      TXString(pBuf,size);
  }
  
}
void inputPlan()
{
  char pBuf[100];
  int size;
  int num=(int)gRx-48;
  int* Flash_ptr;
  Flash_ptr = (int*)0x1040;
  if (gRx==ENTER) TXString("\n\r",2); 
  else TXString(&gRx,1);
  
  switch (gPd.inputStage)
  {
  case 0:
    //new plan now we get the total
    if (num >=0 && num<=9)
    {
      if (gPd.tempInput==-1) //this is first char of the int
      {
       gPd.tempInput=(int)num; 
      }
      else //2nd char or more
      {
        gPd.tempInput*=10;
        gPd.tempInput+=(int)num;
      }
    }
    else if (gRx==ENTER)
    {
      if (gPd.tempInput!=-1)
      {
        //wrap first input (total) and go to next
        if (gPd.tempInput<MAX_LAUNCHES)
        {
           gPd.ArrSize=gPd.tempInput;
           gPd.inputStage=1;
           gPd.tempInput=-1;
        }
        else 
        {
          //reset all and re-start
          gPd.tempInput=-1;
          TXString(gErrorMessage,15);
        }
        
      }
      else //cant skip- enter valid num
      {
          TXString(gErrorMessage,15);

      }
    }
    else // unvalid char (not number and not '\n')
    {
          TXString(gErrorMessage,15);

    }
  break;
 
  
  case 1: /// input numbers. if entered ':' than the number u have is minutes- make it seconds. if \n is the input than ad the number (as seconds) to total
  
      //new plan now we get the total
    if (num >=0 && num<=9)
    {
      if (gPd.tempInput==-1) //this is first char of the int
      {
        gPd.tempInput=(int)num; 
      }
      else //2nd char or more
      {
        gPd.tempInput*=10;
        gPd.tempInput+=(int)num;
      }
    }
    else if (gRx==':')
    {
      if (gPd.tempInput!=-1)
      {
        gPd.minInput=0;
        gPd.minInput = gPd.tempInput*60;
        gPd.tempInput=-1;
      }
        else 
        {
          size = sprintf(pBuf,"OMG noobz, enter a number before pressing ':'\n\r");
          TXString(pBuf,size);
        }      //error enter normal minutes
    }  
    else if (gRx==ENTER)
    {
      if (gPd.tempInput!=-1)
      {
        gPd.PlanArray[gPd.ArrInd]=gPd.minInput+gPd.tempInput;
        gPd.tempInput=-1;
        gPd.minInput=0;
        gPd.ArrInd++;

        if (gPd.ArrInd==gPd.ArrSize)
        {
          //finished
          gCurrentState=E_MAIN;
          menu(gCurrentState);
          gPd.inputStage=2;
          /* writing to flash section */
          FCTL3= FWKEY;
          FCTL1= FWKEY+ERASE;
          *Flash_ptr = 0;
          FCTL1 = FWKEY+WRT;
          Flash_memory[0] = gPd.ArrSize;
          Flash_memory[1] = gPd.inputStage;
          for (gPd.ArrInd=0; gPd.ArrInd<gPd.ArrSize; gPd.ArrInd++)
          {
            Flash_memory[gPd.ArrInd+2]=gPd.PlanArray[gPd.ArrInd];
          }
          FCTL1 = FWKEY;
          FCTL3 = FWKEY+LOCK;
        }
      }
      else 
      {
        TXString(gErrorMessage,15);

      }//cant skip- enter valid num
    }
    else // invalid char (not number and not '\n')
    {
      TXString(gErrorMessage,15);
    }
  break;
  
  case 2:
    if (gRx == 'y' || gRx== 'Y')
    {
      gPd.inputStage=0;
      newplan();
    }
    else
    {
      gCurrentState=E_MAIN;
      menu(gCurrentState);
    }
  break;    
  } 
}
void triggerLaunch()
{
  char pBuf[100];
  int size;
  SendFrame();
  size = sprintf(pBuf,"LAUNCHED#");
  TXString(pBuf,size);
  size = numToChar(pBuf,gPd.ArrInd+1);
  TXString(pBuf,size);
  TXString("\n\r",2);
  gPd.ArrInd++;
  if (gPd.ArrInd == gPd.ArrSize)
  {
    gRunMode=0;
    gPd.ArrInd=0;
    gCurrentState=E_MAIN;
    menu(gCurrentState);
  }
}
void newplan()
{
  char pBuf[100];
  int size; 
  if (gPd.inputStage==2)
  {
    size = sprintf(pBuf,"erase plan?\n\r(Y)es\n\r(N)o\n\r");
    TXString(pBuf,size);
  }
  else
  {
    gPd.inputStage=0;
    gPd.ArrInd=0;
    gPd.ArrSize=0;
    gPd.tempInput=-1;
    size = sprintf(pBuf,"\n\rPlease enter the total launches:\n\r");
    TXString(pBuf,size);  
  }

}
int numToChar(char *str,int n) 
{
  const char digits[] = "0123456789";
  char *p1, *p2;
  int size=0;
  
  do 
  {
    str[size++] = digits[n%10];//base 10
  }while ((n /= 10) > 0);
  
  str[size] = '\0';
  
  for (p1 = str, p2 = str + size - 1; p2 > p1; ++p1, --p2) {
    *p1 ^= *p2;
    *p2 ^= *p1;
    *p1 ^= *p2;
  }
  
  return size;
}

void Launch()
{
  if (gRunMode== 1) BSP_TOGGLE_LED1();
  if (gTimerBCnt == gPd.PlanArray[gPd.ArrInd])
  {
      char pBuf[5];
      int size;
      size=numToChar(pBuf,(gPd.ArrInd+1));
      TXString(pBuf,size); 
      TXString("\n\r",2);
    if (gRunMode==2)
    {
      SendFrame();
    }
    gPd.ArrInd++;
    
    if (gPd.ArrInd == gPd.ArrSize)
    {
      gPd.ArrInd = 0;
      gTimerBCnt= 0;
      gRunMode=0;
      gCurrentState=E_MAIN;
      menu(gCurrentState);
    }
  }
  gTimerBCnt++;

}
void beginLaunch(uint8_t runMode)
{
  int i;
  gPd.ArrInd = 0;
  gTimerBCnt= 0;
  
  if(runMode == 2 && gIsConnected==0)
  {
    TXString("Cant launch. Launcher not connected!\r\n",38);
    gCurrentState= E_MAIN;
    menu(gCurrentState);
  }
  else
  {
    gRunMode=runMode;
    gPd.ArrSize = Flash_memory[0];
    gPd.inputStage = Flash_memory[1];
    for(i=0;i<gPd.ArrSize;i++)
      gPd.PlanArray[i] = Flash_memory[i+2];
    TBCCR0 = 0;
    TBCCR0 = TIMER_B_ONE_SEC;
  }
}
void listenJoin()
{
  if (sJoinSem && (sNumCurrentPeers < NUM_CONNECTIONS))
  {
    SMPL_LinkListen(&sLID[sNumCurrentPeers]);
    sNumCurrentPeers++;
    if (sJoinSem)
    {
      sJoinSem--;
    }
      TXString("LAUNCHER CONNECTED!\n\r",21);
    gIsConnected=1;
    gTimeout=0;
      //setReadyTask(E_LISTEN_JOIN);
  }
}

void runLaunch()
{
    if (gRunMode!=0)
      Launch();
}

/*------------------------------------------------------------------------------
* Initialize task scheduler
------------------------------------------------------------------------------*/
void TaskInit(void)
{
  taskArr[E_RUN_UART].funcPtr = runUART; //HIGHEST PRIORITY- the user always have the ability to stop the system
  taskArr[E_RUN_UART].state = E_SUSPENDED;
  //taskArr[0].priority = DEFAULT_PRIORITY;
  taskArr[E_RUN_TMP_INPUT].funcPtr = runTmpInput; // decision making about input. can wait after launch
  taskArr[E_RUN_TMP_INPUT].state = E_SUSPENDED;
  //taskArr[1].priority = DEFAULT_PRIORITY;
  taskArr[E_RUN_LAUNCH].funcPtr = runLaunch; // launch one firework. should be called immidietly.
  taskArr[E_RUN_LAUNCH].state = E_SUSPENDED;
  //taskArr[2].priority = DEFAULT_PRIORITY;
  taskArr[E_LISTEN_JOIN].funcPtr = listenJoin; // will listen to connections.
  taskArr[E_LISTEN_JOIN].state = E_SUSPENDED;
  taskArr[E_LISTEN_FRAME].funcPtr = listenFrame;
  taskArr[E_LISTEN_FRAME].state = E_SUSPENDED;
}
void listenFrame()
{
  
  if (gIsConnected==0)
    TXString("LAUNCHER CONNECTED!\n\r",21);
  gTimeout=0;
  gIsConnected=1;
  if (sPeerFrameSem)
  {
    uint8_t     msg[MAX_APP_PAYLOAD], len;
    
    // process all frames waiting
    if (SMPL_Receive(sLID[0], msg, &len) == SMPL_SUCCESS)
    {
      ioctlRadioSiginfo_t sigInfo;
      sigInfo.lid = sLID[0];
      SMPL_Ioctl(IOCTL_OBJ_RADIO, IOCTL_ACT_RADIO_SIGINFO, (void *)&sigInfo);
      if(len == ACTION_MSG_SIZE) // drop message if len is not 8
      {
        /*printing of temprature section */
        int temp = msg[6] + (msg[7]<<8);
        char temp_string[] = {" XX.XC\r\n"};
        if( temp < 0 )
        {
          temp_string[0] = '-';
          temp = temp * -1;
        }
        else if( ((temp/1000)%10) != 0 )
        {
          temp_string[0] = '0'+((temp/1000)%10);
        }
        temp_string[4] = '0'+(temp%10);
        temp_string[2] = '0'+((temp/10)%10);
        temp_string[1] = '0'+((temp/100)%10);
       if (temp >= H_THRESH)
       {
         TXString("HIGH TEMPRATURE ON LAUNCHER!!\n\r",31);
          abort();
       }
        TXString("temp: ",6);
        TXString(temp_string,8);
      }
      sPeerFrameSem--;
    }
  }
}
void SendFrame()
{
  uint8_t msg[ACTION_MSG_SIZE];
  for(int i=0; i< ACTION_MSG_SIZE-1; i++)
    msg[i] = gLaunchMsg[i];
  msg[ACTION_MSG_SIZE-1]=gPd.ArrInd+1;
  if (SMPL_SUCCESS == SMPL_Send(sLID[0], msg, ACTION_MSG_SIZE))
  {
    BSP_TOGGLE_LED2();
  }
  else
  {
    BSP_TOGGLE_LED2();
    BSP_TOGGLE_LED1();
  }
}
void abort()
{ 
  TXString("System Locked, press Reset to restart\n\r",39);
  SendAbortFrame();
  BSP_TURN_ON_LED2();                  //Green LED is off
  BSP_TURN_ON_LED1();
  gSystemAbort = 1;
  TACCTL0 &= ~CCIE;
  TBCCTL0 &= ~CCIE;
  IE2 &= ~UCA0RXIE;
  __bis_SR_register(CPUOFF + GIE);
}
void Port_Init()
{
/*
* Configuration of Port 1
* BIT0 - Red onboard LED
* BIT1 - Green onboard LED
* BIT2 (BUTTON) on board button (pulled down)
*/  
   P1SEL &= 0x0;                                  // 0- I/O function is selected
   P1DIR &= 0x0;                                  // Port 1 input direction (BUTTON)
   P1DIR |= BIT0+BIT1;                            // P1.0 and P1.1 output direction (LEDs)  
   P1REN |= BUTTON;                               // P1.2 pullup/pulldown resistor Enabled
   P1OUT |= BUTTON;                               // P1.2 set to logic high level
   P1IES |= BUTTON;                               // P1.2 high-to-low transition
   P1IFG &= ~BUTTON;                              // P1.2 clear any pendings interupts
   P1IE &= 0x0;                                   // Disable all other interupts on Port 1
   P1IE |= BUTTON;                                // P1.2 interupt enable
   
   
}
void setReadyTask(Priorities task)
{
  taskArr[task].state = E_READY;
  gSleepSem++;
}
void createRandomAddress()
{
  unsigned int rand, rand2;
  do
  {
    rand = TI_getRandomIntegerFromVLO();    // first byte can not be 0x00 of 0xFF
  }
  while( (rand & 0xFF00)==0xFF00 || (rand & 0xFF00)==0x0000 );
  rand2 = TI_getRandomIntegerFromVLO();
  
  BCSCTL1 = CALBC1_1MHZ;                    // Set DCO to 1MHz
  DCOCTL = CALDCO_1MHZ;
  FCTL2 = FWKEY + FSSEL0 + FN1;             // MCLK/3 for Flash Timing Generator
  FCTL3 = FWKEY + LOCKA;                    // Clear LOCK & LOCKA bits
  FCTL1 = FWKEY + WRT;                      // Set WRT bit for write operation
  
  Flash_Addr[0]=(rand>>8) & 0xFF;
  Flash_Addr[1]=rand & 0xFF;
  Flash_Addr[2]=(rand2>>8) & 0xFF; 
  Flash_Addr[3]=rand2 & 0xFF; 
  
  FCTL1 = FWKEY;                            // Clear WRT bit
  FCTL3 = FWKEY + LOCKA + LOCK;             // Set LOCK & LOCKA bit
}
void SendAbortFrame()
{
  uint8_t msg[ACTION_MSG_SIZE];
  for(int i=0; i< ACTION_MSG_SIZE-1; i++)
    msg[i] = gAbortMsg[i];
  if (SMPL_SUCCESS == SMPL_Send(sLID[0], msg, ACTION_MSG_SIZE))
  {
    BSP_TOGGLE_LED2();
  }
  else
  {
    BSP_TOGGLE_LED2();
    BSP_TOGGLE_LED1();
  }
}